#include "cloudprocessdepend.h"
#include <QDebug>


CloudProcessDepend::CloudProcessDepend(QObject *parent) : QObject(parent)
{
    cloud.reset(new PointCloudT);
}

void CloudProcessDepend::InputCloud(pcl::PointCloud<pcl::PointXYZ>& input_cloud)
{
    if(input_cloud.empty())
    {
        return;
    }
    this->cloud->clear();
    pcl::copyPointCloud(input_cloud,*this->cloud);
}

// 计算两个向量的点积
float CloudProcessDepend::point_product(Vector3D a, Vector3D b) {
    return (a.x * b.x + a.y * b.y + a.z * b.z);
}

// 向量模长
float CloudProcessDepend::vector_module(Vector3D a)
{
    return std::sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
}

// 按照指定向量方向排序
bool CloudProcessDepend::compareByDirection(Point3D a, Point3D b, Vector3D direction) {
    Vector3D pa = { a.x, a.y, a.z };
    Vector3D pb = { b.x, b.y, b.z };

    //沿指定向量方向的投影
    float proj_a = point_product(pa, direction) / vector_module(direction);
    float proj_b = point_product(pb, direction) / vector_module(direction);
    return proj_a < proj_b; // 升序排序
}

bool CloudProcessDepend::compareByX(const Point3D& a, const Point3D& b) {
    return a.x < b.x; // 升序排序
}

bool CloudProcessDepend::compareByY(const CloudProcessDepend::Point3D &a, const CloudProcessDepend::Point3D &b)
{
    return a.y < b.y; // 升序排序
}

void CloudProcessDepend::CuttingCloudPlanarOnceSlice(std::vector<pcl::PointCloud<pcl::PointXYZ>>& extract_slices_NURBS,std::vector<pcl::PointCloud<pcl::PointXYZ>>& extract_slices_points,
                                                     std::vector<pcl::PointCloud<pcl::Normal>>& extrace_slices_NURBS_normals, std::vector<std::vector<Pose_Vector>>& extrace_slices_NURBS_direction_vectors)

{
    if(this->cloud->empty()) return;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clone(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::copyPointCloud(*cloud, *cloud_clone);

    extract_slices_NURBS.clear();      //保存切片点云
    extract_slices_points.clear();     //保存提取点坐标
    extrace_slices_NURBS_normals.clear();//保存提取点坐标法向量

    float dist = this->cutting_distance;

    /** 点云滤波 **/
    FilteringCloud(cloud_clone, 15, 200);

    /** 体素降采样 **/
    DownsampleCloud(cloud_clone, 1);

    /** 获取点云PCA主方向 **/
    PCAStructure pcastructure = GetPCADirection(cloud_clone);

    for (int count = 0; count < 1; ++count) {
        //ax+by+cz+d=0的平面模型
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());

        //平面法向量为 PCA 第3个主方向
        Eigen::Vector3f normal_vector = pcastructure.eigenVectorsPCA.col(2);

        //平面经过的点
        Eigen::Vector3f point_on_plane;

        //沿选择的主方偏移
        point_on_plane[0] = pcastructure.centriod.x + normal_vector[0] * (dist);
        point_on_plane[1] = pcastructure.centriod.y + normal_vector[1] * (dist);
        point_on_plane[2] = pcastructure.centriod.z + normal_vector[2] * (dist);

        // 计算平面方程的偏移量d
        float d = -(normal_vector.dot(point_on_plane));
        coefficients->values.resize(4);				 //ax+by+cz+d=0
        coefficients->values[0] = normal_vector[0];  //a
        coefficients->values[1] = normal_vector[1];  //b
        coefficients->values[2] = normal_vector[2];  //c
        coefficients->values[3] = d;			     //d

        //归一化
        double det = sqrt(coefficients->values[0] * coefficients->values[0] + coefficients->values[1] * coefficients->values[1] + coefficients->values[2] * coefficients->values[2]);
        coefficients->values[0] = coefficients->values[0] / det;
        coefficients->values[1] = coefficients->values[1] / det;
        coefficients->values[2] = coefficients->values[2] / det;
        coefficients->values[3] = coefficients->values[3] / det;

        //切割点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
        float distance_threshold = ComputeDensity(cloud_clone) * project_threshold;		//设定切割距离阈值
        PlanarSegmentationByDistance(cloud_clone, cloud_plane, *coefficients, distance_threshold);

        //点云投影
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::ProjectInliers<pcl::PointXYZ> projector;
        projector.setModelCoefficients(coefficients);
        projector.setInputCloud(cloud_plane);
        projector.filter(*cloud_projected);

        if(cloud_projected->empty()){
            qDebug() << "cloud_projected size empty";
            continue;
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_NURBS(new pcl::PointCloud<pcl::PointXYZ>());
        std::vector<Point3D> TargetPoints;
        std::vector<Point3D> point3D;
        for (size_t i = 0; i < cloud_projected->size(); ++i)
        {
            Point3D point3d;
            point3d.x = (*cloud_projected)[i].x;
            point3d.y = (*cloud_projected)[i].y;
            point3d.z = (*cloud_projected)[i].z;
            TargetPoints.push_back(point3d);
        }

        for(size_t ii=0; ii<TargetPoints.size(); ii++)
        {
            for(size_t jj =0; jj<TargetPoints.size()-ii-1; jj++)
            {
                Point3D a = TargetPoints[jj];
                Point3D b = TargetPoints[jj+1];
                if(sort_direction == 0)
                {
                    if(!compareByX(a, b))
                    {
                        Point3D temp_point = TargetPoints[jj];
                        TargetPoints[jj] = TargetPoints[jj+1];
                        TargetPoints[jj+1] = temp_point;
                    }
                }
                else if(sort_direction == 1)
                {
                    if(!compareByY(a, b))
                    {
                        Point3D temp_point = TargetPoints[jj];
                        TargetPoints[jj] = TargetPoints[jj+1];
                        TargetPoints[jj+1] = temp_point;
                    }
                }
            }
        }


        //3次NURBS曲线拟合，等弧长法得到曲线上的离散点 cloud_NURBS
        if (GetNURBS(TargetPoints, point3D, 3))
        {
            for (size_t i = 0; i < point3D.size(); ++i)
            {
                pcl::PointXYZ tempoint;
                tempoint.x = point3D[i].x;
                tempoint.y = point3D[i].y;
                tempoint.z = point3D[i].z;
                cloud_NURBS->push_back(tempoint);
            }
            qDebug() << "point3D.size(): " << point3D.size();
        }
        else
        {
            cout << "GetNURBS ERROR!" << endl;
            continue;
        }

        //计算NURBS的切线方向
        std::vector<Eigen::Vector3f> cloud_NURBS_Tangentdirections;
        ComputeNURBSTangentdirection(cloud_NURBS, cloud_NURBS_Tangentdirections);

        //抽取NURBS曲线上的离散点和切线
        int num_sample = this->extract_points_NUM-1;
        int step_size = cloud_NURBS->size() / num_sample;
        pcl::PointCloud<pcl::PointXYZ> cloud_extract;
        std::vector<Eigen::Vector3f> NURBS_Tangentdirections;
        for (size_t i = 0; i < cloud_NURBS->size(); i += step_size) {
            cloud_extract.push_back((*cloud_NURBS)[i]);
            NURBS_Tangentdirections.push_back(cloud_NURBS_Tangentdirections[i]);
        }
        extract_slices_NURBS.push_back(cloud_extract);			//保存NURBS曲线上的点

        //合并曲线上的点与原点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_merged_NURBS(new pcl::PointCloud<pcl::PointXYZ>());
        std::vector<int> indices(cloud_extract.size());			// 创建索引向量，用于存储每个点在合并后点云中曲线上点的索引
        pcl::copyPointCloud(*cloud_clone, *cloud_merged_NURBS);
        for (size_t i = 0; i < cloud_extract.size(); i++) {
            cloud_merged_NURBS->push_back((cloud_extract)[i]);
            indices[i] = cloud_clone->size() + i;					// cloud_clone合并点索引加上cloud_extract的大小
        }

        //计算提取点法线
        pcl::PointCloud<pcl::Normal>::Ptr cloud_merged_NURBS_normals(new pcl::PointCloud<pcl::Normal>());
        pcl::PointCloud<pcl::Normal>::Ptr NURBS_normals(new pcl::PointCloud<pcl::Normal>());
        ComputeNormals(cloud_merged_NURBS, cloud_merged_NURBS_normals);
        for (size_t i = 0; i < cloud_extract.size(); i++) {
            NURBS_normals->push_back((*cloud_merged_NURBS_normals)[indices[i]]);
        }

        //沿法线方向提离高度
        pcl::PointCloud<pcl::PointXYZ>::Ptr NURBS_cloud_extend(new pcl::PointCloud<pcl::PointXYZ>());
        NURBSExtendHeightByNormals(cloud_extract, NURBS_normals, NURBS_cloud_extend, 1.0f);

        //提取点位姿向量
        std::vector<Pose_Vector> NURBS_direction_vectors;
        for (size_t i = 0; i < NURBS_cloud_extend->size(); i++) {
            Eigen::Vector3f Normaldirection;
            Eigen::Vector3f Y_direction;

            //提取Z方向位姿
            Normaldirection[0] = (*NURBS_normals)[i].normal_x * -1;
            Normaldirection[1] = (*NURBS_normals)[i].normal_y * -1;
            Normaldirection[2] = (*NURBS_normals)[i].normal_z * -1;
            double det = std::sqrt(Normaldirection[0] * Normaldirection[0] + Normaldirection[1] * Normaldirection[1] + Normaldirection[2] * Normaldirection[2]);
            Normaldirection[0] = Normaldirection[0] / det;
            Normaldirection[1] = Normaldirection[1] / det;
            Normaldirection[2] = Normaldirection[2] / det;

            //y方向位姿取z与x的叉乘方向
            Y_direction = Normaldirection.cross(NURBS_Tangentdirections[i]);

            Pose_Vector direction_vectors;
            direction_vectors.X_Vector = NURBS_Tangentdirections[i];
            direction_vectors.Y_Vector = Y_direction;
            direction_vectors.Z_Vector = Normaldirection;

            NURBS_direction_vectors.push_back(direction_vectors);
        }

        extract_slices_points.push_back(*NURBS_cloud_extend);
        extrace_slices_NURBS_normals.push_back(*NURBS_normals);
        extrace_slices_NURBS_direction_vectors.push_back(NURBS_direction_vectors);
    }
}

void CloudProcessDepend::CuttingCloudPlanarOnceSlice(std::vector<pcl::PointCloud<pcl::PointXYZ>>& extract_slices_NURBS,std::vector<pcl::PointCloud<pcl::PointXYZ>>& extract_slices_points,
                                                     std::vector<pcl::PointCloud<pcl::Normal>>& extrace_slices_NURBS_normals, std::vector<std::vector<Pose_Vector>>& extrace_slices_NURBS_direction_vectors, pcl::PointXYZ ref_point)

{
    if(this->cloud->empty()) return;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clone(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::copyPointCloud(*cloud, *cloud_clone);

    extract_slices_NURBS.clear();      //保存切片点云
    extract_slices_points.clear();     //保存提取点坐标
    extrace_slices_NURBS_normals.clear();//保存提取点坐标法向量

    /** 点云滤波 **/
    FilteringCloud(cloud_clone, 15, 200);

    /** 体素降采样 **/
    DownsampleCloud(cloud_clone, 1);

    /** 获取点云PCA主方向 **/
    PCAStructure pcastructure = GetPCADirection(cloud_clone);

    for (int count = 0; count < 1; ++count) {
        //ax+by+cz+d=0的平面模型
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());

        //平面法向量为 PCA 第3个主方向
        Eigen::Vector3f normal_vector = pcastructure.eigenVectorsPCA.col(2);

        Eigen::Vector3f ref_point_vector;
        ref_point_vector[0] = ref_point.x - pcastructure.centriod.x;
        ref_point_vector[1] = ref_point.y - pcastructure.centriod.y;
        ref_point_vector[2] = ref_point.z - pcastructure.centriod.z;

        float pointproduct = ref_point_vector[0] * normal_vector[0] + ref_point_vector[1] * normal_vector[1] + ref_point_vector[2] * normal_vector[2];

        float projdistance = pointproduct / std::sqrt((normal_vector[0]* normal_vector[0] + normal_vector[1]* normal_vector[1] + normal_vector[2]* normal_vector[2]));

        float dist = projdistance;

        //平面经过的点
        Eigen::Vector3f point_on_plane;

        //沿选择的主方偏移
        point_on_plane[0] = pcastructure.centriod.x + normal_vector[0] * (dist);
        point_on_plane[1] = pcastructure.centriod.y + normal_vector[1] * (dist);
        point_on_plane[2] = pcastructure.centriod.z + normal_vector[2] * (dist);

        // 计算平面方程的偏移量d
        float d = -(normal_vector.dot(point_on_plane));
        coefficients->values.resize(4);				 //ax+by+cz+d=0
        coefficients->values[0] = normal_vector[0];  //a
        coefficients->values[1] = normal_vector[1];  //b
        coefficients->values[2] = normal_vector[2];  //c
        coefficients->values[3] = d;			     //d

        //归一化
        double det = sqrt(coefficients->values[0] * coefficients->values[0] + coefficients->values[1] * coefficients->values[1] + coefficients->values[2] * coefficients->values[2]);
        coefficients->values[0] = coefficients->values[0] / det;
        coefficients->values[1] = coefficients->values[1] / det;
        coefficients->values[2] = coefficients->values[2] / det;
        coefficients->values[3] = coefficients->values[3] / det;

        //切割点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
        float distance_threshold = ComputeDensity(cloud_clone) * project_threshold;		//设定切割距离阈值
        PlanarSegmentationByDistance(cloud_clone, cloud_plane, *coefficients, distance_threshold);

        //点云投影
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::ProjectInliers<pcl::PointXYZ> projector;
        projector.setModelCoefficients(coefficients);
        projector.setInputCloud(cloud_plane);
        projector.filter(*cloud_projected);

        if(cloud_projected->empty()){
            qDebug() << "cloud_projected size empty";
            continue;
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_NURBS(new pcl::PointCloud<pcl::PointXYZ>());
        std::vector<Point3D> TargetPoints;
        std::vector<Point3D> point3D;
        for (size_t i = 0; i < cloud_projected->size(); ++i)
        {
            Point3D point3d;
            point3d.x = (*cloud_projected)[i].x;
            point3d.y = (*cloud_projected)[i].y;
            point3d.z = (*cloud_projected)[i].z;
            TargetPoints.push_back(point3d);
        }

        for(size_t ii=0; ii<TargetPoints.size(); ii++)
        {
            for(size_t jj =0; jj<TargetPoints.size()-ii-1; jj++)
            {
                Point3D a = TargetPoints[jj];
                Point3D b = TargetPoints[jj+1];
                if(sort_direction == 0)
                {
                    if(!compareByX(a, b))
                    {
                        Point3D temp_point = TargetPoints[jj];
                        TargetPoints[jj] = TargetPoints[jj+1];
                        TargetPoints[jj+1] = temp_point;
                    }
                }
                else if(sort_direction == 1)
                {
                    if(!compareByY(a, b))
                    {
                        Point3D temp_point = TargetPoints[jj];
                        TargetPoints[jj] = TargetPoints[jj+1];
                        TargetPoints[jj+1] = temp_point;
                    }
                }
            }
        }


        //3次NURBS曲线拟合，等弧长法得到曲线上的离散点 cloud_NURBS
        if (GetNURBS(TargetPoints, point3D, 3))
        {
            for (size_t i = 0; i < point3D.size(); ++i)
            {
                pcl::PointXYZ tempoint;
                tempoint.x = point3D[i].x;
                tempoint.y = point3D[i].y;
                tempoint.z = point3D[i].z;
                cloud_NURBS->push_back(tempoint);
            }
            qDebug() << "point3D.size(): " << point3D.size();
        }
        else
        {
            cout << "GetNURBS ERROR!" << endl;
            continue;
        }

        //计算NURBS的切线方向
        std::vector<Eigen::Vector3f> cloud_NURBS_Tangentdirections;
        ComputeNURBSTangentdirection(cloud_NURBS, cloud_NURBS_Tangentdirections);

        //抽取NURBS曲线上的离散点和切线
        int num_sample = this->extract_points_NUM-1;
        int step_size = cloud_NURBS->size() / num_sample;
        pcl::PointCloud<pcl::PointXYZ> cloud_extract;
        std::vector<Eigen::Vector3f> NURBS_Tangentdirections;
        for (size_t i = 0; i < cloud_NURBS->size(); i += step_size) {
            cloud_extract.push_back((*cloud_NURBS)[i]);
            NURBS_Tangentdirections.push_back(cloud_NURBS_Tangentdirections[i]);
        }
        extract_slices_NURBS.push_back(cloud_extract);			//保存NURBS曲线上的点

        //合并曲线上的点与原点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_merged_NURBS(new pcl::PointCloud<pcl::PointXYZ>());
        std::vector<int> indices(cloud_extract.size());			// 创建索引向量，用于存储每个点在合并后点云中曲线上点的索引
        pcl::copyPointCloud(*cloud_clone, *cloud_merged_NURBS);
        for (size_t i = 0; i < cloud_extract.size(); i++) {
            cloud_merged_NURBS->push_back((cloud_extract)[i]);
            indices[i] = cloud_clone->size() + i;					// cloud_clone合并点索引加上cloud_extract的大小
        }

        //计算提取点法线
        pcl::PointCloud<pcl::Normal>::Ptr cloud_merged_NURBS_normals(new pcl::PointCloud<pcl::Normal>());
        pcl::PointCloud<pcl::Normal>::Ptr NURBS_normals(new pcl::PointCloud<pcl::Normal>());
        ComputeNormals(cloud_merged_NURBS, cloud_merged_NURBS_normals);
        for (size_t i = 0; i < cloud_extract.size(); i++) {
            NURBS_normals->push_back((*cloud_merged_NURBS_normals)[indices[i]]);
        }

        //沿法线方向提离高度
        pcl::PointCloud<pcl::PointXYZ>::Ptr NURBS_cloud_extend(new pcl::PointCloud<pcl::PointXYZ>());
        NURBSExtendHeightByNormals(cloud_extract, NURBS_normals, NURBS_cloud_extend, 1.0f);

        //提取点位姿向量
        std::vector<Pose_Vector> NURBS_direction_vectors;
        for (size_t i = 0; i < NURBS_cloud_extend->size(); i++) {
            Eigen::Vector3f Normaldirection;
            Eigen::Vector3f Y_direction;

            //提取Z方向位姿
            Normaldirection[0] = (*NURBS_normals)[i].normal_x * -1;
            Normaldirection[1] = (*NURBS_normals)[i].normal_y * -1;
            Normaldirection[2] = (*NURBS_normals)[i].normal_z * -1;
            double det = std::sqrt(Normaldirection[0] * Normaldirection[0] + Normaldirection[1] * Normaldirection[1] + Normaldirection[2] * Normaldirection[2]);
            Normaldirection[0] = Normaldirection[0] / det;
            Normaldirection[1] = Normaldirection[1] / det;
            Normaldirection[2] = Normaldirection[2] / det;

            //y方向位姿取z与x的叉乘方向
            Y_direction = Normaldirection.cross(NURBS_Tangentdirections[i]);

            Pose_Vector direction_vectors;
            direction_vectors.X_Vector = NURBS_Tangentdirections[i];
            direction_vectors.Y_Vector = Y_direction;
            direction_vectors.Z_Vector = Normaldirection;

            NURBS_direction_vectors.push_back(direction_vectors);
        }

        extract_slices_points.push_back(*NURBS_cloud_extend);
        extrace_slices_NURBS_normals.push_back(*NURBS_normals);
        extrace_slices_NURBS_direction_vectors.push_back(NURBS_direction_vectors);
    }
}

/** 提取多个点云切片法向量方向坐标 **/
void CloudProcessDepend::CuttingCloudPlanarSlices(std::vector<pcl::PointCloud<pcl::PointXYZ>>& extract_slices_NURBS,std::vector<pcl::PointCloud<pcl::PointXYZ>>& extract_slices_points,
                                                  std::vector<pcl::PointCloud<pcl::Normal>>& extrace_slices_NURBS_normals, std::vector<std::vector<CloudProcessDepend::Pose_Vector>>& extrace_slices_NURBS_direction_vectors)
{
    if(this->cloud->empty()) return;

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_clone(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::copyPointCloud(*cloud, *cloud_clone);

    extract_slices_NURBS.clear();      //保存切片点云
    extract_slices_points.clear();     //保存提取点坐标
    extrace_slices_NURBS_normals.clear();//保存提取点坐标法向量

    float dist = this->cutting_distance;
    int slice_NUM = this->cutting_NUM;

    /** 点云滤波 **/
    FilteringCloud(cloud_clone, 15, 200);

    /** 体素降采样 **/
    DownsampleCloud(cloud_clone, 1);

    /** 获取点云PCA主方向 **/
    PCAStructure pcastructure = GetPCADirection(cloud_clone);

    int sdd_1 = 0; int sdd_2 = 1;
    for (int count = 0; count < slice_NUM; ++count) {
        //ax+by+cz+d=0的平面模型
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());

        //平面法向量为 PCA 第3个主方向
        Eigen::Vector3f normal_vector = pcastructure.eigenVectorsPCA.col(2);

        //平面经过的点
        Eigen::Vector3f point_on_plane;

        //沿选择的主方两侧偏移
        if (count % 2 == 0)
        {
            point_on_plane[0] = pcastructure.centriod.x + normal_vector[0] * (dist * sdd_1);
            point_on_plane[1] = pcastructure.centriod.y + normal_vector[1] * (dist * sdd_1);
            point_on_plane[2] = pcastructure.centriod.z + normal_vector[2] * (dist * sdd_1);
            sdd_1 += 1;
        }
        else
        {
            point_on_plane[0] = pcastructure.centriod.x - normal_vector[0] * (dist * sdd_2);
            point_on_plane[1] = pcastructure.centriod.y - normal_vector[1] * (dist * sdd_2);
            point_on_plane[2] = pcastructure.centriod.z - normal_vector[2] * (dist * sdd_2);
            sdd_2 += 1;
        }

        // 计算平面方程的偏移量d
        float d = -(normal_vector.dot(point_on_plane));
        coefficients->values.resize(4);				 //ax+by+cz+d=0
        coefficients->values[0] = normal_vector[0];  //a
        coefficients->values[1] = normal_vector[1];  //b
        coefficients->values[2] = normal_vector[2];  //c
        coefficients->values[3] = d;			     //d

        //归一化
        double det = sqrt(coefficients->values[0] * coefficients->values[0] + coefficients->values[1] * coefficients->values[1] + coefficients->values[2] * coefficients->values[2]);
        coefficients->values[0] = coefficients->values[0] / det;
        coefficients->values[1] = coefficients->values[1] / det;
        coefficients->values[2] = coefficients->values[2] / det;
        coefficients->values[3] = coefficients->values[3] / det;

        //切割点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
        float distance_threshold = ComputeDensity(cloud_clone) * project_threshold;		//设定切割距离阈值
        PlanarSegmentationByDistance(cloud_clone, cloud_plane, *coefficients, distance_threshold);

        //点云投影
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>());
        pcl::ProjectInliers<pcl::PointXYZ> projector;
        projector.setModelCoefficients(coefficients);
        projector.setInputCloud(cloud_plane);
        projector.filter(*cloud_projected);

        if(cloud_projected->empty()){
            qDebug() << "cloud_projected size empty";
            continue;
        }

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_NURBS(new pcl::PointCloud<pcl::PointXYZ>());
        std::vector<Point3D> TargetPoints;
        std::vector<Point3D> point3D;
        for (size_t i = 0; i < cloud_projected->size(); ++i)
        {
            Point3D point3d;
            point3d.x = (*cloud_projected)[i].x;
            point3d.y = (*cloud_projected)[i].y;
            point3d.z = (*cloud_projected)[i].z;
            TargetPoints.push_back(point3d);
        }

        //按照指定向量方向排序，向量方向指定为第二个PCA主方向
        //        Vector3D sortDirection{ pcastructure.pcY.x, pcastructure.pcY.y, pcastructure.pcY.z };	//向量方向指定为第二个PCA主方向
        //        for(size_t ii=0; ii<TargetPoints.size(); ii++){
        //            for(size_t jj =0; jj<TargetPoints.size()-ii-1; jj++){
        //                Point3D a = TargetPoints[jj];
        //                Point3D b = TargetPoints[jj+1];
        //                if(!compareByDirection(a, b, sortDirection)){
        //                    Point3D temp_point = TargetPoints[jj];
        //                    TargetPoints[jj] = TargetPoints[jj+1];
        //                    TargetPoints[jj+1] = temp_point;
        //                }
        //            }
        //        }

        for(size_t ii=0; ii<TargetPoints.size(); ii++){
            for(size_t jj =0; jj<TargetPoints.size()-ii-1; jj++){
                Point3D a = TargetPoints[jj];
                Point3D b = TargetPoints[jj+1];
                if(!compareByX(a, b)){
                    Point3D temp_point = TargetPoints[jj];
                    TargetPoints[jj] = TargetPoints[jj+1];
                    TargetPoints[jj+1] = temp_point;
                }
            }
        }


        //3次NURBS曲线拟合，等弧长法得到曲线上的离散点 cloud_NURBS
        if (GetNURBS(TargetPoints, point3D, 3))
        {
            for (size_t i = 0; i < point3D.size(); ++i)
            {
                pcl::PointXYZ tempoint;
                tempoint.x = point3D[i].x;
                tempoint.y = point3D[i].y;
                tempoint.z = point3D[i].z;
                cloud_NURBS->push_back(tempoint);
            }
            qDebug() << "point3D.size(): " << point3D.size();
        }
        else
        {
            cout << "GetNURBS ERROR!" << endl;
            continue;
        }

        //计算NURBS的切线方向
        std::vector<Eigen::Vector3f> cloud_NURBS_Tangentdirections;
        ComputeNURBSTangentdirection(cloud_NURBS, cloud_NURBS_Tangentdirections);

        //抽取NURBS曲线上的离散点和切线
        int num_sample = this->extract_points_NUM-1;
        int step_size = cloud_NURBS->size() / num_sample;
        pcl::PointCloud<pcl::PointXYZ> cloud_extract;
        std::vector<Eigen::Vector3f> NURBS_Tangentdirections;
        for (size_t i = 0; i < cloud_NURBS->size(); i += step_size) {
            cloud_extract.push_back((*cloud_NURBS)[i]);
            NURBS_Tangentdirections.push_back(cloud_NURBS_Tangentdirections[i]);
        }
        extract_slices_NURBS.push_back(cloud_extract);			//保存NURBS曲线上的点

        //合并曲线上的点与原点云
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_merged_NURBS(new pcl::PointCloud<pcl::PointXYZ>());
        std::vector<int> indices(cloud_extract.size());			// 创建索引向量，用于存储每个点在合并后点云中曲线上点的索引
        pcl::copyPointCloud(*cloud_clone, *cloud_merged_NURBS);
        for (size_t i = 0; i < cloud_extract.size(); i++) {
            cloud_merged_NURBS->push_back((cloud_extract)[i]);
            indices[i] = cloud_clone->size() + i;					// cloud_clone合并点索引加上cloud_extract的大小
        }

        //计算提取点法线
        pcl::PointCloud<pcl::Normal>::Ptr cloud_merged_NURBS_normals(new pcl::PointCloud<pcl::Normal>());
        pcl::PointCloud<pcl::Normal>::Ptr NURBS_normals(new pcl::PointCloud<pcl::Normal>());
        ComputeNormals(cloud_merged_NURBS, cloud_merged_NURBS_normals);
        for (size_t i = 0; i < cloud_extract.size(); i++) {
            NURBS_normals->push_back((*cloud_merged_NURBS_normals)[indices[i]]);
        }

        //沿法线方向提离高度
        pcl::PointCloud<pcl::PointXYZ>::Ptr NURBS_cloud_extend(new pcl::PointCloud<pcl::PointXYZ>());
        NURBSExtendHeightByNormals(cloud_extract, NURBS_normals, NURBS_cloud_extend, 1.0f);

        //提取点位姿向量
        std::vector<Pose_Vector> NURBS_direction_vectors;
        for (size_t i = 0; i < NURBS_cloud_extend->size(); i++) {
            Eigen::Vector3f Normaldirection;
            Eigen::Vector3f Y_direction;

            //提取Z方向位姿
            Normaldirection[0] = (*NURBS_normals)[i].normal_x * -1;
            Normaldirection[1] = (*NURBS_normals)[i].normal_y * -1;
            Normaldirection[2] = (*NURBS_normals)[i].normal_z * -1;
            double det = std::sqrt(Normaldirection[0] * Normaldirection[0] + Normaldirection[1] * Normaldirection[1] + Normaldirection[2] * Normaldirection[2]);
            Normaldirection[0] = Normaldirection[0] / det;
            Normaldirection[1] = Normaldirection[1] / det;
            Normaldirection[2] = Normaldirection[2] / det;

            //y方向位姿取z与x的叉乘方向
            Y_direction = Normaldirection.cross(NURBS_Tangentdirections[i]);

            Pose_Vector direction_vectors;
            direction_vectors.X_Vector = NURBS_Tangentdirections[i];
            direction_vectors.Y_Vector = Y_direction;
            direction_vectors.Z_Vector = Normaldirection;

            NURBS_direction_vectors.push_back(direction_vectors);
        }

        extract_slices_points.push_back(*NURBS_cloud_extend);
        extrace_slices_NURBS_normals.push_back(*NURBS_normals);
        extrace_slices_NURBS_direction_vectors.push_back(NURBS_direction_vectors);
    }
}


/** 计算NURBS曲线点切线方向 **/
void CloudProcessDepend::ComputeNURBSTangentdirection(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_NURBS, std::vector<Eigen::Vector3f>& cloud_NURBS_Tangentdirection)
{
    cloud_NURBS_Tangentdirection.clear();
    for (size_t i = 0; i < cloud_NURBS->size()-1; ++i) {
        pcl::PointXYZ point_A = (*cloud_NURBS)[i];
        pcl::PointXYZ point_B = (*cloud_NURBS)[i+1];

        Eigen::Vector3f tangentdirection;
        tangentdirection[0] = point_B.x - point_A.x;
        tangentdirection[1] = point_B.y - point_A.y;
        tangentdirection[2] = point_B.z - point_A.z;

        //归一化
        double det = sqrt(tangentdirection[0] * tangentdirection[0] + tangentdirection[1] * tangentdirection[1] + tangentdirection[2] * tangentdirection[2]);
        tangentdirection[0] = tangentdirection[0] / det;
        tangentdirection[1] = tangentdirection[1] / det;
        tangentdirection[2] = tangentdirection[2] / det;

        cloud_NURBS_Tangentdirection.push_back(tangentdirection);
    }
    Eigen::Vector3f last_tangentdirection = cloud_NURBS_Tangentdirection.back();
    cloud_NURBS_Tangentdirection.push_back(last_tangentdirection);
}

/** 计算点云法线 **/
void CloudProcessDepend::ComputeNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::Normal>::Ptr& normals)
{
    // 计算点云法线
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> m;		// OMP加速
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    m.setNumberOfThreads(8);		// 设置OpenMP线程数
    m.setInputCloud(cloud);
    m.setSearchMethod(tree);
    m.setKSearch(50);				// 计算法向量近邻大小
    m.compute(*normals);			// 开始法向量计算
    m.setViewPoint(0, 0, 0);		// 设置视点
    normal_flip(cloud, normals, 0);	// 翻转点云法线为同一方向
}

/** 翻转点云法线为同一方向 **/
void CloudProcessDepend::normal_flip(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::Normal>::Ptr& cloud_normals, int seed_index)
{
    std::vector<pcl::PointXYZ> points;
    std::vector<pcl::Normal> normals;
    points.push_back(cloud->points[seed_index]);
    normals.push_back(cloud_normals->points[seed_index]);

    std::vector<bool> flags(cloud->size(), false);   //用来标记点云中的点是否被生长过
    flags[seed_index] = true;

    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    kdtree.setInputCloud(cloud);
    int K = 20;
    std::vector<int> pointsIdx(K);          //索引
    std::vector<float> pointsDistance(K);   //距离

    while (!normals.empty())
    {
        pcl::PointXYZ seed_point = points.back();   //种子点
        pcl::Normal seed_normal = normals.back();   //种子点法线
        points.pop_back();
        normals.pop_back();

        kdtree.nearestKSearch(seed_point, K, pointsIdx, pointsDistance);    //k近邻搜索
        Eigen::Vector3f v1(seed_normal.normal_x, seed_normal.normal_y, seed_normal.normal_z);

        for (size_t i = 0; i < pointsIdx.size(); i++)
        {
            if (!flags[pointsIdx[i]])   //如果该点没有被生长到
            {
                Eigen::Vector3f	v2(cloud_normals->points[pointsIdx[i]].normal_x,
                        cloud_normals->points[pointsIdx[i]].normal_y,
                        cloud_normals->points[pointsIdx[i]].normal_z);

                if (v1.dot(v2) < 0) //如果该点法线方向与种子点法线方向相反（夹角为钝角），则翻转法线方向
                {
                    cloud_normals->points[pointsIdx[i]].normal_x *= -1;
                    cloud_normals->points[pointsIdx[i]].normal_y *= -1;
                    cloud_normals->points[pointsIdx[i]].normal_z *= -1;
                }
                points.push_back(cloud->points[pointsIdx[i]]);
                normals.push_back(cloud_normals->points[pointsIdx[i]]);
                flags[pointsIdx[i]] = true;    //标记该点已经被生长过
            }
        }
    }
}

void CloudProcessDepend::NURBSExtendHeightByNormals(pcl::PointCloud<pcl::PointXYZ> NURBS_cloud, pcl::PointCloud<pcl::Normal>::Ptr& NURBS_normals, pcl::PointCloud<pcl::PointXYZ>::Ptr& extend_cloud, float height_increment)
{
    // 沿着法线方向增加高度
    for (size_t i = 0; i < NURBS_cloud.size(); ++i)
    {
        pcl::PointXYZ point = NURBS_cloud.at(i);
        pcl::Normal normal = (*NURBS_normals)[i];

        // 更新点的位置沿着法线方向
        point.x += normal.normal_x * height_increment;
        point.y += normal.normal_y * height_increment;
        point.z += normal.normal_z * height_increment;

        extend_cloud->push_back(point);
    }
}

/** 无序点云曲面重建 **/
//贪婪投影三角化算法对有向点云三角化处理，可处理表面光滑连续、密度均匀的无序化点云
void CloudProcessDepend::DisorderedPointCloudsSurfaceRestruction(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PolygonMesh& triangles)
{
    // Normal estimation
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>());
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud);
    n.setInputCloud(cloud);
    n.setSearchMethod(tree);
    n.setKSearch(50);
    n.compute(*normals);

    // Concatenate the XYZ and normal fields
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);

    // Create search tree*
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>);
    tree2->setInputCloud(cloud_with_normals);

    // Initialize objects
    //pcl::GreedyProjectionTriangulation<pcl::PointNormal> m_gp3;

    //    float density = ComputeDensity(cloud);

    //    // Set the maximum distance between connected points (maximum edge length)
    //    m_gp3.setSearchRadius(density * 5.0);				// 三角形最大边长

    //    // Set typical values for the parameters
    //    m_gp3.setMu(density * 100.0);				// 样本点搜索邻域最远距离
    //    m_gp3.setMaximumNearestNeighbors(100);	// 样本点可搜索邻域个数
    //    m_gp3.setMaximumSurfaceAngle(M_PI / 4);	// 45 degrees 偏离样本点法线方向最大角度 45度
    //    m_gp3.setMinimumAngle(M_PI / 18);			// 10 degrees 三角化后内角最小角度 10度
    //    m_gp3.setMaximumAngle(2 * M_PI / 3);		// 120 degrees 三角化后内角最大角度 120度
    //    m_gp3.setNormalConsistency(false);		// 设置保证法线朝向一致

    //    // Get result
    //    m_gp3.setInputCloud(cloud_with_normals);
    //    m_gp3.setSearchMethod(tree2);
    //    m_gp3.reconstruct(triangles);

    //    // Additional vertex information
    //    std::vector<int> parts = m_gp3.getPartIDs();
    //    std::vector<int> states = m_gp3.getPointStates();
}

/** 泊松重建 **/
void CloudProcessDepend::PoissonSurfaceReconstruction(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PolygonMesh& output_mesh)
{
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>());
    ComputeNormals(cloud,normals);

    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
    pcl::concatenateFields(*cloud, *normals, *cloud_with_normals);

    //泊松重建
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal>);
    tree->setInputCloud(cloud_with_normals);
    pcl::Poisson<pcl::PointNormal> pn;
    pn.setSearchMethod(tree);
    pn.setInputCloud(cloud_with_normals);
    pn.setDepth(20);             // 设置将用于表面重建的树的最大深度
    pn.setMinDepth(10);
    pn.setScale(0.48);           // 设置用于重建的立方体的直径与样本的边界立方体直径的比值
    pn.setSolverDivide(3);       // 设置块高斯-塞德尔求解器用于求解拉普拉斯方程的深度。
    pn.setIsoDivide(6);          // 设置块等表面提取器用于提取等表面的深度
    pn.setSamplesPerNode(50);    // 设置每个八叉树节点上最少采样点数目
    pn.setConfidence(false);     // 设置置信标志，为true时，使用法线向量长度作为置信度信息，false则需要对法线进行归一化处理
    pn.setManifold(false);       // 设置流行标志，如果设置为true，则对多边形进行细分三角话时添加重心，设置false则不添加
    pn.setOutputPolygons(false); // 设置是否输出为多边形(而不是三角化行进立方体的结果)

    pn.performReconstruction(output_mesh);
}

/** 生成NURBS曲线 **/
bool CloudProcessDepend::GetNURBS(const std::vector<Point3D>& TargetPoints, std::vector<Point3D>& point3D, int k)
{
    std::vector<double> x, y, z, U;
    Eigen::MatrixXd dpt1(1, 3), dptn(1, 3);
    double Sum = 0.0;
    int n = TargetPoints.size();
    Eigen::RowVectorXd pt1;
    Eigen::RowVectorXd pt_n;
    Eigen::MatrixXd pt(n, 3);
    Eigen::MatrixXd A(n, n);
    Eigen::MatrixXd E(n, 3);
    Eigen::MatrixXd dU(1, n + k + 3);			 //节点增量，△U = Ui + 1 - Ui
    Eigen::MatrixXd D(n, 3);
    Eigen::MatrixXd CtrlPts(n + 2, 3);
    Eigen::VectorXd w(CtrlPts.rows());			 // 权重向量（所有控制点权重相同默认赋值1）
    struct InputsStruct inputstruct;
    w.setOnes();
    pt.setZero();

    Eigen::RowVectorXd Time_Array(TargetPoints.size() + 2);		//控制节点比型值点多2个
    for (int i = 0; i < TargetPoints.size(); i++)				//填充时间向量
    {
        Time_Array(i) = i;
    }

    for (size_t i = 0; i < TargetPoints.size(); ++i) {
        x.push_back(TargetPoints[i].x);
        y.push_back(TargetPoints[i].y);
        z.push_back(TargetPoints[i].z);
        pt(i, 0) = TargetPoints[i].x;
        pt(i, 1) = TargetPoints[i].y;
        pt(i, 2) = TargetPoints[i].z;
    }

    /** 计算控制点 **/
    // 计算累积弦长
    U.resize(n + k + 3, 0);
    std::vector<double> tempdists;
    for (size_t i = 0; i < n - 1; ++i) {
        double dx = x[i + 1] - x[i];
        double dy = y[i + 1] - y[i];
        double dz = z[i + 1] - z[i];
        double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
        tempdists.push_back(dist);
        Sum += dist;
    }

    // 设置 U 向量的边界条件
    std::fill(U.begin(), U.begin() + k, 0.0);
    std::fill(U.begin() + n + k - 1, U.end(), 1.0);

    // 计算 U 向量的其他元素
    for (size_t i = k; i < n + k - 2; ++i) {
        U[i + 1] = U[i] + tempdists[i - k] / Sum;
    }

    dpt1(0, 0) = 0; dpt1(0, 1) = 0; dpt1(0, 2) = 1;
    dptn(0, 0) = -1; dptn(0, 1) = 0; dptn(0, 2) = 0;

    dU.setZero();
    for (size_t i = k; i < n + k - 1; i++) {
        dU(0, i) = U[i + 1] - U[i];
    }

    A.setZero();
    A(0, 0) = 1.0;				// 切矢条件 a1=1, b1=c1=0
    A(n - 1, n - 1) = 1.0;		// 切矢条件 an=bn=0, cn=1

    pt1 = pt.row(0);
    pt_n = pt.row(n - 1);

    E.setZero();
    // 首端点条件
    E.row(0) = pt1 + (dU(0, 3) / 3.0) * dpt1;
    // 末端点条件
    E.row(n - 1) = pt_n - (dU(0, n + 1) / 3.0) * dptn;

    // 计算系数矩阵 A 的元素 a, b, c 以及列向量 E 的元素 e 的值
    for (int i = 1; i < n - 1; ++i) {
        double du_i_1_2_3 = dU(0, i + 1) + dU(0, i + 2) + dU(0, i + 3);
        double du_i_1_2 = dU(i + 1) + dU(i + 2);
        double du_i_3_4 = dU(i + 3) + dU(i + 4);
        double du_i_2_3_4 = dU(i + 2) + dU(i + 3) + dU(i + 4);
        A(i, i - 1) = dU(i + 3) * dU(i + 3) / du_i_1_2_3;	// a 的值
        A(i, i) = (dU(i + 3) * du_i_1_2 / du_i_1_2_3) + (dU(i + 2) *
                                                         du_i_3_4 / (du_i_2_3_4));						// b 的值
        A(i, i + 1) = dU(i + 2) * dU(i + 2) / (du_i_2_3_4);	// c 的值

        Eigen::RowVectorXd tmep_row = pt.row(i);
        E.row(i) = (dU(i + 2) + dU(i + 3)) * tmep_row;		//e 的值
    }

    // 解线性方程组 A * D = E
    // 检查矩阵 A 是否可逆
    if (A.determinant() == 0) {
        return false;
    }

    // 计算矩阵 A 的逆
    D = A.lu().solve(E);

    CtrlPts.row(0) = pt1;
    CtrlPts.row(n + 1) = pt_n;
    for (int i = 1; i < CtrlPts.rows() - 1; i++) {
        Eigen::RowVectorXd temp_row = D.row(i - 1);
        CtrlPts.row(i) = temp_row;
    }
    //打印控制点数量n+2个控制点
    //cout << "CtrlPts.rows = " << CtrlPts.rows() << endl;

    Eigen::MatrixXd tempmatrix(4, CtrlPts.rows());
    tempmatrix.row(0) = Time_Array;
    tempmatrix.row(1) = CtrlPts.col(0);
    tempmatrix.row(2) = CtrlPts.col(1);
    tempmatrix.row(3) = CtrlPts.col(2);
    Eigen::MatrixXd Time_Ctrlpoints = tempmatrix.transpose();

    //计算插值点个数
    double dst = 0.1;						//NURBS 曲线插值密度
    int PNUM = (CtrlPts.rows() - 1) / dst;

    /** 生成 NURBS 曲线上的点 **/
    inputstruct.data = Time_Ctrlpoints;		// 控制点矩阵
    inputstruct.knots = U;					// 节点向量
    inputstruct.ctrPnums = Time_Ctrlpoints.rows();
    inputstruct.w = w;
    inputstruct.x = Eigen::VectorXd::LinSpaced(PNUM, 0, 1);
    inputstruct.k = 3;

    Eigen::MatrixXd TrajpointsMatrix = MyNURBS(inputstruct);

    for (int i = 0; i < TrajpointsMatrix.rows(); i++) {
        Point3D temp_point3D;
        temp_point3D.x = TrajpointsMatrix(i, 1); temp_point3D.y = TrajpointsMatrix(i, 2); temp_point3D.z = TrajpointsMatrix(i, 3);
        point3D.push_back(temp_point3D);
    }

    return true;
}

/**
    计算给定点 x 对应的基函数值，其中 i 是基函数的索引，k 是曲线的次数
    x		前点的参数值
    knots 	结点向量
    i		基函数的索引
    k		曲线的次数
    **/
double CloudProcessDepend::GetBasisFcn(double x, const std::vector<double>& knots, int i, int k)
{
    // 首先检查曲线的次数是否为0，这是递归的基本情况
    if (k == 0) {
        // 检查参数值 x 是否位于当前基函数的支撑区间内
        if ((x >= knots[i + 1]) && (x < knots[i + 2])) {
            return 1.0;    // 如果 x 在支撑区间内，基函数值为1
        }
        else {
            return 0.0;    // 如果 x 不在支撑区间内，基函数值为0
        }
    }
    else {    // 如果次数不为0，进入递归计算
        double A = GetBasisFcn(x, knots, i, k - 1);      // 递归调用 GetBasisFcn 函数
        double B = GetBasisFcn(x, knots, i + 1, k - 1);

        double w1 = 0.0;
        double w2 = 0.0;

        // 检查递归计算得到的基函数值是否为0，以防止除以0
        if (A != 0) {
            w1 = (x - knots[i + 1]) / (knots[i + k + 1] - knots[i + 1]);
        }

        if (B != 0) {
            w2 = (knots[i + k + 2] - x) / (knots[i + k + 2] - knots[i + 2]);
        }

        // 根据计算得到的权重和基函数值，计算当前基函数的值
        double res = w1 * A + w2 * B;
        return res;
    }
}

/**
    函数通过构建基函数矩阵、计算权重和控制点 来生成曲线上的点
    InputsStruct 作为参数，
    并返回曲线上的点 Q
**/
Eigen::MatrixXd CloudProcessDepend::MyNURBS(const InputsStruct& inputs)
{
    Eigen::MatrixXd ctrP = inputs.data;		//获取结构体中的控制点数据
    Eigen::VectorXd x = inputs.x;			//获取参数化向量 x，它定义了曲线上点的位置
    std::vector<double> knots = inputs.knots;	//获取结点向量
    int k = inputs.k;						//获取曲线的次数
    int ctrPnums = inputs.ctrPnums;			//获取控制点的数量
    Eigen::VectorXd w = inputs.w;			//获取权重向量

    // 初始化基函数矩阵 N
    Eigen::MatrixXd N(x.size(), ctrPnums);
    N.setZero();

    // 循环遍历参数化向量 x 的每个元素
    for (int i = 0; i < x.size(); ++i) {
        Eigen::RowVectorXd y(ctrPnums);
        y.setZero(); // 初始化一个零向量 y

        // 循环遍历所有控制点
        for (int j = 0; j < ctrPnums; ++j) {
            y(j) = GetBasisFcn(x(i), knots, j - 1, k);
        }

        N.row(i) = y; // 将计算得到的基函数值向量赋给基函数矩阵 N 的相应行
    }

    // 计算曲线上的点 Q
    Eigen::MatrixXd wN = N * w.asDiagonal(); // 权重和基函数矩阵的乘积
    Eigen::VectorXd w_transposed = w.transpose();
    Eigen::MatrixXd norm = N * w_transposed; // 计算 N 和 w 的内积
    Eigen::MatrixXd Q(wN.rows(), wN.cols());
    Q.setZero();
    for (size_t i = 0; i < wN.rows(); i++) {
        Eigen::RowVectorXd tempRowMatrix = wN.row(i);
        Eigen::RowVectorXd tempRowMatrix2 = tempRowMatrix.array() / norm(i);
        Q.row(i) = tempRowMatrix2;
    }

    Q = Q * ctrP;

    // 将最后一个控制点作为曲线的终点
    Q.row(Q.rows() - 1) = ctrP.row(ctrP.rows() - 1);

    return Q;
}

/** 计算点云平均点密度 **/
float CloudProcessDepend::ComputeDensity(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    //首先初始化搜索领域半径radius
    float radius = 0.4;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr treedense(new pcl::search::KdTree<pcl::PointXYZ>);
    treedense->setInputCloud(cloud);
    //初始化向量，储存局部密度
    std::vector<int> nn_indices;//储存点云中每个点的最邻索引
    std::vector<float> nn_dists; //储存每个点的最邻距离
    std::vector<float> densiities(cloud->size(), 0);//储存每个点的局部密度，初始化为零

    //nn_in和nn_dists,在算法中用于计算每个点的局部密度，对于每个点通过tree->ra函数找到其在领域半径内的所有最邻点的索引和距离
    for (size_t i = 0; i < cloud->size(); i++) {
        treedense->radiusSearch(i, radius, nn_indices, nn_dists);//1.要查找最邻的点的索引，2.指定半径，3.储存最邻点索引的向量
        densiities[i] = nn_indices.size();//赋值给densities
    }
    float SumDensities = 0;
    for (size_t i = 0; i < densiities.size(); i++) {
        SumDensities += densiities[i];
    }
    float density = SumDensities / cloud->size();

    return density;
}

/** 按距离切割点云 **/
bool CloudProcessDepend::PlanarSegmentationByDistance(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_plane, pcl::ModelCoefficients coefficients, float distance_threshold)
{
    float a = coefficients.values[0];
    float b = coefficients.values[1];
    float c = coefficients.values[2];
    float d = coefficients.values[3];

    //按点到平面的距离阈值滤除点云
    float distance = 0.0;
    for (size_t i = 0; i < cloud->size(); ++i) {
        pcl::PointXYZ point = (*cloud)[i];
        distance = std::abs(a * point.x + b * point.y + c * point.z + d) / std::sqrt(a * a + b * b + c * c);

        if (distance < distance_threshold) {
            cloud_plane->push_back(point);
        }
    }

    if (cloud_plane->size() == 0) {
        return false;
    }

    return true;
}

/** 滤波 **/
void CloudProcessDepend::FilteringCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, float range = 1.5, float Radius = 2)
{
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> outlier_filt;
    outlier_filt.setInputCloud(cloud);              // 设置待滤波的点云
    outlier_filt.setRadiusSearch(range);            // 设置半径搜索范围
    outlier_filt.setMinNeighborsInRadius(Radius);	// 设置半径范围内点的最小数量
    outlier_filt.filter(*cloud);                    // 执行滤波
}

/** 降采样 **/
void CloudProcessDepend::DownsampleCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, float LeafSize)
{
    //VoxelGrid滤波器将原始点云划分为体素（或称为格子、网格），
    //并将每个体素内的点集合并为一个点作为该体素的代表点。
    //体素的大小可以通过设置VoxelGrid滤波器的leaf size参数来控制
    pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
    voxel_grid.setInputCloud(cloud);
    voxel_grid.setLeafSize(LeafSize, LeafSize, LeafSize);		// set voxel size
    voxel_grid.filter(*cloud);                                  // apply filter
}

/** 利用PCA主元分析法获得点云的三个主方向，
获取质心，计算协方差，获得协方差矩阵，
求取协方差矩阵的特征值和特征向量，特征向量即为主方向 **/
CloudProcessDepend::PCAStructure CloudProcessDepend::GetPCADirection(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{
    Eigen::Vector4f pcaCentriod;
    pcl::compute3DCentroid(*cloud, pcaCentriod);
    Eigen::Matrix3f covariance;
    pcl::computeCovarianceMatrixNormalized(*cloud, pcaCentriod, covariance);	//计算点云cloud相对于其质心pcaCentriod的归一化协方差矩阵
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance, Eigen::ComputeEigenvectors);//求解对称矩阵covariance的特征值和特征向量
    Eigen::Matrix3f eigenVectorsPCA = eigen_solver.eigenvectors();	//特征向量矩阵

    //校正主方向间垂直
    eigenVectorsPCA.col(2) = eigenVectorsPCA.col(0).cross(eigenVectorsPCA.col(1)); //通过叉乘计算第三个特征向量，确保三个特征向量相互垂直
    eigenVectorsPCA.col(0) = eigenVectorsPCA.col(1).cross(eigenVectorsPCA.col(2)); //重新计算第一个特征向量，确保它与另外两个特征向量垂直
    eigenVectorsPCA.col(1) = eigenVectorsPCA.col(2).cross(eigenVectorsPCA.col(0)); //重新计算第一个特征向量，确保它与另外两个特征向量垂直

    Eigen::Matrix4f tm = Eigen::Matrix4f::Identity();
    Eigen::Matrix4f tm_inv = Eigen::Matrix4f::Identity();
    tm.block<3, 3>(0, 0) = eigenVectorsPCA.transpose();   //R.	//将eigenVectorsPCA矩阵（包含PCA计算出的特征向量）转置后赋值给tm矩阵的左上角3x3部分，这部分通常表示旋转矩阵R
    tm.block<3, 1>(0, 3) = -1.0f * (eigenVectorsPCA.transpose()) * (pcaCentriod.head<3>());//  -R*t	//计算旋转后的质心（pcaCentriod的前三个分量）并取负值，然后乘以旋转矩阵的转置，得到平移向量。这个平移向量赋值给tm矩阵的左下角3x1部分
    tm_inv = tm.inverse();	//计算变换矩阵tm的逆矩阵

    //将点云数据通过PCA分析得到的主要方向进行旋转和平移变换，以便于对点云进行分析或进一步处理
    pcl::PointCloud<PointType>::Ptr transformedCloud(new pcl::PointCloud<PointType>);
    pcl::transformPointCloud(*cloud, *transformedCloud, tm);

    PointType min_p1, max_p1;
    Eigen::Vector3f c1, c;
    pcl::getMinMax3D(*transformedCloud, min_p1, max_p1);
    c1 = 0.5f * (min_p1.getVector3fMap() + max_p1.getVector3fMap());	//计算质心

    Eigen::Affine3f tm_inv_aff(tm_inv);
    pcl::transformPoint(c1, c, tm_inv_aff);

    Eigen::Vector3f whd, whd1;
    whd1 = max_p1.getVector3fMap() - min_p1.getVector3fMap();
    whd = whd1;

    //计算点云平均尺度，这是通过将点云的宽度、高度和深度相加然后除以3得到的
    float sc1 = (whd1(0) + whd1(1) + whd1(2)) / 3;  //点云平均尺度，用于设置主方向箭头大小

    //变换到原点的点云主方向
    PointType op;
    op.x = 0.0;
    op.y = 0.0;
    op.z = 0.0;
    Eigen::Vector3f px, py, pz;
    Eigen::Affine3f tm_aff(tm);
    pcl::transformVector(eigenVectorsPCA.col(0), px, tm_aff);
    pcl::transformVector(eigenVectorsPCA.col(1), py, tm_aff);
    pcl::transformVector(eigenVectorsPCA.col(2), pz, tm_aff);
    PointType pcaX;
    pcaX.x = sc1 * px(0);
    pcaX.y = sc1 * px(1);
    pcaX.z = sc1 * px(2);
    PointType pcaY;
    pcaY.x = sc1 * py(0);
    pcaY.y = sc1 * py(1);
    pcaY.z = sc1 * py(2);
    PointType pcaZ;
    pcaZ.x = sc1 * pz(0);
    pcaZ.y = sc1 * pz(1);
    pcaZ.z = sc1 * pz(2);

    //初始点云的主方向
    PointType cp;
    cp.x = pcaCentriod(0);
    cp.y = pcaCentriod(1);
    cp.z = pcaCentriod(2);
    PointType pcX;
    pcX.x = sc1 * eigenVectorsPCA(0, 0) + cp.x;
    pcX.y = sc1 * eigenVectorsPCA(1, 0) + cp.y;
    pcX.z = sc1 * eigenVectorsPCA(2, 0) + cp.z;
    PointType pcY;
    pcY.x = sc1 * eigenVectorsPCA(0, 1) + cp.x;
    pcY.y = sc1 * eigenVectorsPCA(1, 1) + cp.y;
    pcY.z = sc1 * eigenVectorsPCA(2, 1) + cp.z;
    PointType pcZ;
    pcZ.x = sc1 * eigenVectorsPCA(0, 2) + cp.x;
    pcZ.y = sc1 * eigenVectorsPCA(1, 2) + cp.y;
    pcZ.z = sc1 * eigenVectorsPCA(2, 2) + cp.z;

    CloudProcessDepend::PCAStructure pcastructure;
    pcastructure.centriod = cp;
    pcastructure.pcX = pcX;
    pcastructure.pcY = pcY;
    pcastructure.pcZ = pcZ;
    pcastructure.eigenVectorsPCA = eigenVectorsPCA;

    return pcastructure;
}
